'''
Created on 2011-07-04

@author: Earl Bell
'''

#import Tkinter as Tk
from Tkinter import *
import OSEKitsDefine 
from math import *
import datetime

import matplotlib
import tkMessageBox
#matplotlib.use('TkAgg')

from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg, NavigationToolbar2TkAgg
from matplotlib.figure import Figure
import numpy


class OSEEncoder(object):
    '''
    Class: OSEEncoder
    
    Description:
        This class handles the use of the encoder sensor.  Contains functions to parse measurement packets, re-draw
        the display, and configure the sensors.
    
    '''
    _measured_times = []
    _measured_positions = []
    _measured_velocitys = []
    _measured_accelerations = []
    _smoothed_velocitys = []
    _smoothed_accelerations = []
    #diameter/teeth
    #(d*pi)/n_teeth
    _pulse_distance = 0.038 * pi / 18
    def __init__(self):
        '''
        Constructor
        '''
        self._encoder_teeth = StringVar()
        self._pulley_diameter = StringVar()

    def save_data(self, file_type, file):
        '''
        Function: save_data
        
        Description:
            Takes data from last run thermocouple experiment and saves it to either a csv or xml file.
            
        Parameters:
            String file_type: "csv" store data in csv format (tab delimited)
                              "xml" store data in xml format
            File file: an already opened file where the data is to be stored.
        
        Returns:
            String: "success" if no errors encountered.
                    "<error message>" otherwise
        '''
        
        if file_type == "csv":
            try:
                now = datetime.datetime.now()
                current_time= now.strftime("%H:%M %Y-%m-%d")
                file.write("OSEKits Encoder Data\t")
                file.write(current_time)
                file.write("\n")
                file.write("Elapsed Time (s)\tDisplacement(m)\tMeasured Velocity(m/s)\tSmoothed Velocity(m/s)\tMeasured Acceleration(m/s^2)\tSmoothed Acceleration(m/s^2)\n")
                for i in range(0, len(self._measured_times)):
                    file.write(str(self._measured_times[i]) + 
                               '\t' + str(self._measured_positions[i]) + 
                               '\t' + str(self._measured_velocitys[i]) + 
                               '\t' + str(self._smoothed_velocitys[i]) +
                               '\t' + str(self._measured_accelerations[i]) +
                               '\t' + str(self._smoothed_accelerations[i]) +
                               '\n')
                file.close()
            except Exception, e:
                return "File error: " + e
        elif file_type == "xml":
            try:
                file.write("""<?xml version="1.0" encoding="UTF-8" ?>\n""")
                file.write("<OSEData>\n")
                file.write("<title>OSEKits Encoder Data</title>\n")
                now = datetime.datetime.now()
                current_time= now.strftime("%H:%M %Y-%m-%d")
                file.write("<datetime>")
                file.write(current_time)
                file.write("</datetime>\n")
                for i in range(0, len(self._measured_times)):
                    file.write("<measurement>\n")
                    file.write("\t<elapsedtime>" + str(self._measured_times[i]) +"</elapsedtime>\n"+ 
                               "\t<measuredposition>" + str(self._measured_positions[i]) + "</measuredposition>\n" + 
                               "\t<measuredvelocity>" + str(self._measured_velocitys[i]) + "</measuredvelocity>\n" +
                               "\t<smoothedvelocity>" + str(self._smoothed_velocitys[i]) + "</smoothedvelocity>\n" +
                               "\t<measuredaccelerations>" + str(self._measured_accelerations[i]) + "</measuredaccelerations>\n" +
                               "\t<smoothedaccelerations>" + str(self._smoothed_accelerations[i]) + "</smoothedaccelerations>\n")
                    file.write("</measurement>\n")
                file.write("</OSEData>")
                file.close()
            except Exception, e:
                return "File error: " + e
                
        return "success"


        
    def start_measurement(self):
        '''
        Function: start_measurement
        
        Description:
            Called when starting to take measurements from the micro-controller. Initializes measurement storage
            and sets appropriate flags and counters.
            
        Returns:
            string  "success" when no errors encountered
                    "<error message>" otherwise
        '''
        
        self._measured_times = []
        self._measured_positions = []
        self._measured_velocitys = []
        self._measured_accelerations = []
        self._smoothed_velocitys = []
        self._smoothed_accelerations = []        
        self._first_time = 0

        try:
            pulley_diameter = float(self._pulley_diameter.get()) / 1000
            encoder_teeth = int(self._encoder_teeth.get())
            self._pulse_distance = pulley_diameter * pi / encoder_teeth
        except:
            tkMessageBox.showerror("Data entry Error", "The pulley diameter or the number of encoder teeth is invalid.\nExperiment Aborted")
            return "Data Entry Error"
        
        try:
            self._position_figure.clear()
        except:
            #return "Position graph clear error"
            #continue
            wait = 1
        
        
        try:
            self._velocity_figure.clear()
        except:
            #return "Velocity graph clear error"
            wait = 1
        
        
        try:
            self._acceleration_figure.clear()
        except:
            #return"Acceleration graph clear error"
            wait = 1
        
        
        return "success"
        
    def stop_measurement(self):
        '''
        Function: stop_measurement
        
        Description:
            Called when done taking measurements from the micro-controller. Calculates the displacement,
            velocity, and acceleration measurements.
            
        Returns:
            string  "success" when no errors encountered
                    "<error message>" otherwise
        '''        
        #calculate positions
        for i in range(0, len(self._measured_times)):
            self._measured_positions.append(self._pulse_distance * i )
            
        #calculate Velocities
        self._measured_velocitys.append(0)
        for i in range(1, len(self._measured_times)):
            change_in_time = self._measured_times[i] - self._measured_times[i-1]
            
            if change_in_time <> 0:
                self._measured_velocitys.append(self._pulse_distance / change_in_time)
            else:
                self._measured_velocitys.append(0)
        
        self._smoothed_velocitys = self.smoothListGaussian(self._measured_velocitys, False, 10)        


        #calculate Accelerations
        self._measured_accelerations.append(0)                        
        self._measured_accelerations.append(0)
        
        for i in range(2, len(self._measured_times)):
            change_in_time = (self._measured_times[i] - self._measured_times[i-1])
            change_in_velocity = (self._smoothed_velocitys[i] - self._smoothed_velocitys[i-1])

            if change_in_time <> 0:
                self._measured_accelerations.append(change_in_velocity / change_in_time)
            else:
                self._measured_accelerations.append(0)    
        
        self._smoothed_accelerations = self.smoothListGaussian(self._measured_accelerations, False, 10)
        

        

        #redraw plots

        try:
            self._update_graphs()
        except Exception, e:
            tkMessageBox.showerror("Data Error", "Bad data has been reported from the microcontroller.\n Re-try experiment.")       
        return
       
    def smoothListGaussian(self, list, strippedXs=False, degree=5):
        '''
        Function: smoothListGaussian
        
        Description:  
            Takes in a List of float values and smoothes them over a window of size degree.
            http://www.swharden.com/blog/2008-11-17-linear-data-smoothing-in-python/
        
        Parameters:
            List: list: a List of float values
            Boolean: strippedXs:  unused in this version
            Integer: degree:  the size of the smoothing window.  
        Returns:
            List of Floats
        
        '''
        list = [list[0]]*(degree-1) + list + [list[-1]]*degree  
        window=degree*2-1  
        weight=numpy.array([1.0]*window)  
        weightGauss=[]  
        for i in range(window):  
            i=i-degree+1  
            frac=i/float(window)  
            gauss=1/(numpy.exp((4*(frac))**2))  
            weightGauss.append(gauss)  
        weight=numpy.array(weightGauss)*weight  
        smoothed=[0.0]*(len(list)-window)  
        for i in range(len(smoothed)):  
            smoothed[i]=sum(numpy.array(list[i:i+window])*weight)/sum(weight)  
        return smoothed  
        
 
    def add_measurement(self, measurement_packet):
        '''
        Function: add_measurement
        
        Description: 
            Takes in a measurement packet and converts the packet into usable data. Stores data
            in a list. 
        
        Parameters: 
            string  a string containing all characters sent from the micro-controller
            
        Returns:
            string  "success" if no errors encountered
                    "<error message>" otherwise
        '''
        shift = 0
        measurement_data = 0

        for char in measurement_packet:
            measurement_data = measurement_data | ord(char) << 8 * shift
            shift = shift + 1        

        if len(self._measured_times) > 0:
            self._measured_times.append((float(measurement_data) / 1000000) - self._first_time)
            
        else:
            self._measured_times.append(0.0)
            self._first_time = float(measurement_data) / 1000000
        print "measured:", self._measured_times[-1]
        return
        
        
   
  
    def _update_graphs(self):
        '''
        Function: _update_graphs
        
        Description:
            Re-plots the three graphs in the main display.
            
        Returns:
            Nothing
        '''
        self._position_figure.add_subplot(self._position_plot)
        self._position_plot.plot(self._measured_times,self._measured_positions)
        self._position_plot.set_title('Displacement vs Time')
        self._position_plot.set_xlabel('Time (s)')
        self._position_plot.set_ylabel('Displacement (m)')
        self._position_figure.subplots_adjust(top=0.9, bottom = 0.15, right = 0.9, left = 0.125)
        self.position_display.show()

        self._velocity_figure.clear()        
        self._velocity_figure.add_subplot(self._smoothed_velocity_plot)
        self._smoothed_velocity_plot.plot(self._measured_times,self._smoothed_velocitys)
        self._smoothed_velocity_plot.set_title('Velocity vs Time')
        self._smoothed_velocity_plot.set_xlabel('Time (s)')
        self._smoothed_velocity_plot.set_ylabel('Velocity (m/s)')
        self._velocity_figure.subplots_adjust(top=0.9, bottom = 0.15, right = 0.9, left = 0.125)
        
        self.velocity_display.show()

        self._acceleration_figure.clear()
        self._acceleration_figure.add_subplot(self._smoothed_acceleration_plot)
        self._smoothed_acceleration_plot.plot(self._measured_times,self._smoothed_accelerations)
        self._smoothed_acceleration_plot.set_title('Acceleration vs Time')
        self._smoothed_acceleration_plot.set_xlabel('Time (s)')
        self._smoothed_acceleration_plot.set_ylabel('Acceleration (m/s^2)')
        self._acceleration_figure.subplots_adjust(top=0.9, bottom = 0.15, right = 0.9, left = 0.125)
        
        
        
        self.acceleration_display.show()
        return
        
    def setup_display(self, MainWindow):
        '''
        Function: setup_display
        
        Description:
            Adds all display elements to the main window.
            
        Parameters:
            Tkinter.Tk   The main tk window object where the display will be altered
            
        Returns:
            Nothing
        '''        
        self._smoothed_accelerations = []
        self._measured_accelerations = []
        self._measured_positions = []
        self._measured_velocitys =[]
        self._measured_times =[]
        self._smoothed_velocitys =[]
        OSEKitsDefine.CONTROL_ROW = OSEKitsDefine.FIRST_DISPLAY_ROW
        pad_y_start = 90
        
        self.number_teeth_label = Label(MainWindow, text = "# of Encoder Teeth:")
        self.number_teeth_label.pack()
        self.number_teeth_label.grid(column = 0, row=OSEKitsDefine.FIRST_DISPLAY_ROW, sticky = S+W, pady = pad_y_start)
        
        self.number_teeth_entry = Spinbox(MainWindow, background = "white", increment = 1, repeatdelay = 200, repeatinterval = 200, from_ = 1, to_ = 100, textvariable = self._encoder_teeth)
        self.number_teeth_entry.pack()
        self.number_teeth_entry.grid(column = 0, row = OSEKitsDefine.FIRST_DISPLAY_ROW, sticky = S+W, pady =pad_y_start -20)

        self.pulley_size_label = Label(MainWindow, text = "Diameter of Pulley (mm):")
        self.pulley_size_label.pack()
        self.pulley_size_label.grid(column = 0, row=OSEKitsDefine.FIRST_DISPLAY_ROW, sticky = S+W, pady = pad_y_start-40)
        
        self.pulley_size_entry = Spinbox(MainWindow, background = "white", increment = 1, repeatdelay = 200, repeatinterval = 200, from_ = 1, to_ = 300, textvariable = self._pulley_diameter)
        self.pulley_size_entry.pack()
        self.pulley_size_entry.grid(column = 0, row = OSEKitsDefine.FIRST_DISPLAY_ROW, sticky = S+W, pady = pad_y_start - 60)
        
        self._encoder_teeth.set("18")
        self._pulley_diameter.set("38")
        
        self._position_figure = Figure(figsize=(6,3), dpi=100)
        self._position_plot = self._position_figure.add_subplot(111)
        self._position_plot.plot(self._measured_times,self._measured_positions)
        
        self.position_display = FigureCanvasTkAgg(self._position_figure, master=MainWindow)
        self.position_display.show()
        self.position_display.get_tk_widget().grid()
        self.position_display._tkcanvas.grid(column = 1, row = OSEKitsDefine.FIRST_DISPLAY_ROW, sticky =S+W, padx = 10)
        
        self._velocity_figure = Figure(figsize=(6,3), dpi=100)
        self._smoothed_velocity_plot = self._velocity_figure.add_subplot(111)
        self._smoothed_velocity_plot.plot(self._measured_times,self._smoothed_velocitys)
        
        self.velocity_display = FigureCanvasTkAgg(self._velocity_figure, master=MainWindow)
        self.velocity_display.show()
        self.velocity_display.get_tk_widget().grid()
        self.velocity_display._tkcanvas.grid(column = 1, row = OSEKitsDefine.FIRST_DISPLAY_ROW+1, sticky =S+W, padx = 10)
        
        self._acceleration_figure = Figure(figsize=(6,3), dpi=100)
        self._smoothed_acceleration_plot = self._acceleration_figure.add_subplot(111)
        self._smoothed_acceleration_plot.plot(self._measured_times,self._smoothed_accelerations)
        
        self.acceleration_display = FigureCanvasTkAgg(self._acceleration_figure, master=MainWindow)
        self.acceleration_display.show()
        self.acceleration_display.get_tk_widget().grid()
        self.acceleration_display._tkcanvas.grid(column = 1, row = OSEKitsDefine.FIRST_DISPLAY_ROW+2, sticky =S+W, padx = 10)
        
        
        
        self._update_graphs()

    
    
        return
  
    def destroy_display(self, MainWindow):
        '''
        Function: destroy_display
        
        Description:
            Removes all display elements from the main window.
            
        Parameters:
            Tkinter.Tk   The main tk window object where the display will be altered
        
        Returns:
            Nothing
        '''
        OSEKitsDefine.CONTROL_ROW = 2
        self.number_teeth_entry.destroy()
        self.number_teeth_label.destroy()
        self.pulley_size_entry.destroy()
        self.pulley_size_label.destroy()
        self.position_display.get_tk_widget().destroy()
        self.position_display._tkcanvas.destroy()
        self.velocity_display.get_tk_widget().destroy()
        self.velocity_display._tkcanvas.destroy()      
        self.acceleration_display.get_tk_widget().destroy()
        self.acceleration_display._tkcanvas.destroy()
        
        return
